// SteeringSystemCAN.cpp
#include "STEERING_SYSTEM_CAN.h"
#include <cstring>
#include <iostream>
#include <chrono>
#include <thread>
#include <sys/ioctl.h>
#include <unistd.h>

// 初始化静态成员变量
TimingThresholds SteeringSystemCAN::currentTimingThresholds = {
    200000,  // output_no_response
    80000,   // output_too_fast
    120000,  // output_too_slow
    400,     // feedback_normal
    500,     // feedback_slow
    5000     // feedback_no_response
};

RPMThresholds SteeringSystemCAN::currentRPMThresholds = {
    -300,    // min_rpm (舵角范围)
    300,     // max_rpm (舵角范围)
    true     // allow_negative (允许负舵角)
};

// SteeringSystemCAN 类的实现
// 设置时间阈值
void SteeringSystemCAN::setTimingThresholds(const TimingThresholds& thresholds) {
    currentTimingThresholds = thresholds;
}

// 获取当前时间阈值
TimingThresholds SteeringSystemCAN::getCurrentThresholds() {
    return currentTimingThresholds;
}

// 设置舵角和舵速阈值
void SteeringSystemCAN::setRPMThresholds(const RPMThresholds& thresholds) {
    currentRPMThresholds = thresholds;
}

// 获取当前舵角和舵速阈值
RPMThresholds SteeringSystemCAN::getCurrentRPMThresholds() {
    return currentRPMThresholds;
}

// 判断输出指令状态
int SteeringSystemCAN::checkOutputTiming(int64_t timeInterval) {
    auto thresholds = getCurrentThresholds();
    if (timeInterval > thresholds.output_no_response) {
        return 3;
    } else if (timeInterval < thresholds.output_too_fast) {
        return 2;
    } else if (timeInterval > thresholds.output_too_slow) {
        return 1;
    }
    return 0;
}

// 判断反馈状态
int SteeringSystemCAN::checkFeedbackTiming(int64_t timeBack) {
    auto thresholds = getCurrentThresholds();
    if (timeBack > thresholds.feedback_no_response) {
        return 2;
    } else if (timeBack > thresholds.feedback_slow) {
        return 1;
    }
    return 0;
}

// 判断舵角和舵速是否正常
bool SteeringSystemCAN::checkSteeringAngleAndSpeed(const uint8_t* data, size_t offset, std::string& message) {
    int16_t angle = parseSteeringAngle(data, offset);
    float speed = parseSteeringSpeed(data, offset + 2);

    if (angle < currentRPMThresholds.min_rpm || angle > currentRPMThresholds.max_rpm) {
        message = "舵角超出允许范围(" + std::to_string(currentRPMThresholds.min_rpm) + "到" + std::to_string(currentRPMThresholds.max_rpm) + ")";
        return false;
    }

    if (speed < 0.0f || speed > 10.0f) {
        message = "舵速超出允许范围(0到10.0 deg/s)";
        return false;
    }

    message = "舵角和舵速正常";
    return true;
}

// 从原始数据解析舵角
int16_t SteeringSystemCAN::parseSteeringAngle(const uint8_t* data, size_t offset) {
    return (data[offset + 1] << 8) | data[offset];
}

// 从原始数据解析舵速
float SteeringSystemCAN::parseSteeringSpeed(const uint8_t* data, size_t offset) {
    uint16_t raw = (data[offset + 1] << 8) | data[offset];
    return raw / 10.0f;  // 舵速精度为0.1 deg/s
}

// 解析收放指令：0=保持, 1=展开, 2=收回, 3=锁紧, 4=解锁
uint8_t SteeringSystemCAN::parseDeployCommand(const uint8_t* data, size_t offset) {
    return data[offset];
}

// 判断是否为超权限模式
bool SteeringSystemCAN::isOverrideEnabled(const uint8_t* data, size_t offset) {
    return data[offset] == 1;
}

// 判断母线电压状态
int SteeringSystemCAN::checkBusbarVoltage(const uint8_t* data, size_t offset) {
    uint16_t voltage = (data[offset + 1] << 8) | data[offset];
    return (voltage > 10000) ? 1 : 0;
}

// 判断电机电流状态
int SteeringSystemCAN::checkMotorCurrent(const uint8_t* data, size_t offset) {
    int16_t current = (data[offset + 1] << 8) | data[offset];
    return (current < -20000 || current > 20000) ? 1 : 0;
}

// 判断温度状态
int SteeringSystemCAN::checkTemperature(const uint8_t* data, size_t offset) {
    return (data[offset] > 255) ? 1 : 0;
}

// 判断故障位状态
int SteeringSystemCAN::checkFaultBits(const uint8_t* data, size_t offset) {
    uint8_t fault1 = data[offset];
    uint8_t fault2 = data[offset + 1];
    uint8_t fault3 = data[offset + 2];
    uint8_t fault4 = data[offset + 3];  // 保留备用

    if (fault1 & 0x01) return 1;  // 舵机超速报警
    if (fault1 & 0x02) return 2;  // 卡舵
    if (fault1 & 0x04) return 3;  // 过流
    if (fault1 & 0x08) return 4;  // 超上限
    if (fault1 & 0x10) return 5;  // 超下限
    if (fault1 & 0x20) return 6;  // 欠压
    if (fault1 & 0x40) return 7;  // 过压
    if (fault1 & 0x80) return 8;  // Motor过温

    if (fault2 & 0x01) return 9;   // IGBT过温
    if (fault2 & 0x02) return 10;  // 漏水
    if (fault2 & 0x04) return 11;  // CAN通信故障
    if (fault2 & 0x08) return 12;  // OPCUA通信故障

    if (fault3 & 0x01) return 13;  // 未展开操舵保护自锁
    if (fault3 & 0x02) return 14;  // 未锁紧操舵保护自锁
    if (fault3 & 0x04) return 15;  // 带舵角展开保护自锁
    if (fault3 & 0x08) return 16;  // 带舵角收回保护自锁
    if (fault3 & 0x10) return 17;  // 未解锁收回保护自锁

    return 0; // 无故障
}

// SteeringController 类的实现
SteeringController::SteeringController(const std::string& ifname) {
    // 初始化随机数生成器
    rng.seed(std::chrono::system_clock::now().time_since_epoch().count());
    // 创建socket
    if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("Socket creation failed");
        exit(EXIT_FAILURE);
    }
    // 指定CAN接口
    strcpy(ifr.ifr_name, ifname.c_str());
    ioctl(sock, SIOCGIFINDEX, &ifr);
    // 绑定socket到CAN接口
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("Bind failed");
        close(sock);
        exit(EXIT_FAILURE);
    }
}

SteeringController::~SteeringController() {
    close(sock);
}

// 生成随机舵角 (99%: -300至300, 1%: 400)
int16_t SteeringController::generateRandomSteering() {
    std::uniform_int_distribution<int> percent_dist(0, 99);
    if (percent_dist(rng) < 99) {  // 99%概率
        std::uniform_int_distribution<int16_t> steering_dist(-300, 300);
        return steering_dist(rng);
    } else {  // 1%概率
        return 400;
    }
}

// 生成随机间隔 (99%: 99-101ms, 1%: <80 or >120, 极少数>200ms)
int SteeringController::generateRandomInterval() {
    std::uniform_int_distribution<int> percent_dist(0, 99);
    int percent = percent_dist(rng);

    if (percent < 99) {  // 99%概率: 99-101ms
        std::uniform_int_distribution<int> normal_dist(99, 101);
        return normal_dist(rng);
    } else {  // 1%概率
        std::uniform_int_distribution<int> extreme_dist(0, 9);
        if (extreme_dist(rng) == 0) {  // 10% of 1% = 0.1%概率 >200ms
            std::uniform_int_distribution<int> very_long_dist(201, 300);
            return very_long_dist(rng);
        } else {  // 90% of 1% = 0.9%概率 <80 or >120
            std::uniform_int_distribution<int> abnormal_dist(0, 1);
            if (abnormal_dist(rng)) {
                std::uniform_int_distribution<int> short_dist(10, 79);
                return short_dist(rng);
            } else {
                std::uniform_int_distribution<int> long_dist(121, 200);
                return long_dist(rng);
            }
        }
    }
}

// 发送舵角命令
void SteeringController::sendSteeringCommand(int16_t steering, bool faultClear) {
    struct can_frame frame;
    memset(&frame, 0, sizeof(frame));
    frame.can_id = 0x334;  // 修正了 CAN ID 的表示
    frame.can_dlc = 8;     // 8字节数据长度

    // 设置舵角(小端格式)
    frame.data[0] = steering & 0xFF;        // 舵角低位
    frame.data[1] = (steering >> 8) & 0xFF; // 舵角高位

    // 故障清零
    frame.data[2] = faultClear ? 0x01 : 0x00;

    // 4-8字节保留(默认为0)

    if (write(sock, &frame, sizeof(frame)) != sizeof(frame)) {
        perror("Write failed");
    } else {
        std::cout << "Sent steering command: " << steering << ", fault clear: " << (faultClear ? "true" : "false") << std::endl;
    }
}

// 运行舵角控制器
void SteeringController::run() {
    while (true) {
        // 生成随机舵角
        int16_t steering = generateRandomSteering();

        // 发送命令（故障位固定为0）
        sendSteeringCommand(steering, false);

        // 获取随机间隔并休眠
        int interval = generateRandomInterval();
        std::this_thread::sleep_for(std::chrono::milliseconds(interval));

        // 打印实际间隔
        std::cout << "Next message in " << interval << "ms" << std::endl;
    }
}